#include "gimbal_ctrl.h"
#include <semaphore.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>
#include <wiringPi.h>
#include <unistd.h>

gimbal_ctrl_t gimbal_ctrl;

/* wiring init */
static void wiring_init(void)
{
    /* 初始化 */
    if (wiringPiSetup() == -1)
    {
        exit (1);
    }

    /* 设置管脚输出方向 */
    pinMode(GIMBAL_PITCH_PIN, OUTPUT);
    pinMode(GIMBAL_YAW_PIN, OUTPUT);

    return;
}

/* 控制云台向上 */
static void gimbal_move_up(void)
{
    /* 计算控制电平 */
    gimbal_ctrl.pitch_pulse -= GIMBAL_SENS_LEVEL;

    if (gimbal_ctrl.pitch_pulse < GIMBAL_MIN_PULSE)
    {
        gimbal_ctrl.pitch_pulse = GIMBAL_MIN_PULSE;
    }

    /* 产生PWM波，驱动电机 */
    digitalWrite(GIMBAL_PITCH_PIN, HIGH);
    usleep(gimbal_ctrl.pitch_pulse);
    digitalWrite(GIMBAL_PITCH_PIN, LOW);
    usleep(GIMBAL_WAVE_PERIOD);

    return;
}

/* 控制云台向下 */
static void gimbal_move_down(void)
{
    /* 计算控制电平 */
    gimbal_ctrl.pitch_pulse += GIMBAL_SENS_LEVEL;

    if (gimbal_ctrl.pitch_pulse > GIMBAL_MAX_PULSE)
    {
        gimbal_ctrl.pitch_pulse = GIMBAL_MAX_PULSE;
    }

    /* 产生PWM波，驱动电机 */
    digitalWrite(GIMBAL_PITCH_PIN, HIGH);
    usleep(gimbal_ctrl.pitch_pulse);
    digitalWrite(GIMBAL_PITCH_PIN, LOW);
    usleep(GIMBAL_WAVE_PERIOD);

    return;
}

/* 控制云台向左 */
static void gimbal_move_left(void)
{
    /* 计算控制电平 */
    gimbal_ctrl.yaw_pulse -= GIMBAL_SENS_LEVEL;

    if (gimbal_ctrl.yaw_pulse < GIMBAL_MIN_PULSE)
    {
        gimbal_ctrl.yaw_pulse = GIMBAL_MIN_PULSE;
    }

    /* 产生PWM波，驱动电机 */
    digitalWrite(GIMBAL_YAW_PIN, HIGH);
    usleep(gimbal_ctrl.yaw_pulse);
    digitalWrite(GIMBAL_YAW_PIN, LOW);
    usleep(GIMBAL_WAVE_PERIOD);

    return;
}

/* 控制云台向右 */
static void gimbal_move_right(void)
{
    /* 计算控制电平 */
    gimbal_ctrl.yaw_pulse += GIMBAL_SENS_LEVEL;

    if (gimbal_ctrl.yaw_pulse > GIMBAL_MAX_PULSE)
    {
        gimbal_ctrl.yaw_pulse = GIMBAL_MAX_PULSE;
    }

    /* 产生PWM波，驱动电机 */
    digitalWrite(GIMBAL_YAW_PIN, HIGH);
    usleep(gimbal_ctrl.yaw_pulse);
    digitalWrite(GIMBAL_YAW_PIN, LOW);
    usleep(GIMBAL_WAVE_PERIOD);

    return;
}

/* 告知线程有控制信号 */
void ctrl_notice_update(void)
{
    sem_post(&gimbal_ctrl.sem);
}

/* 处理运动控制命令 */
void *gimbal_control_process(void *arg)
{
    int  ret;
    char ctrl_order[10];

    /* 初始化 wiring  */
    wiring_init();

    /* 初始化信号量 */
    ret = sem_init(&gimbal_ctrl.sem, 0, 0);

    if (ret != 0) 
    {
        perror("sem init failed\n");
        exit(1);
    }

    /* 初始化互斥锁 */
    ret = pthread_mutex_init(&gimbal_ctrl.lock, NULL); 

    if (ret != 0) 
    {
        perror("mutex init failed\n");
        exit(1);
    }

    /* 初始化控制周期值 (1500us) */
    gimbal_ctrl.pitch_pulse = GIMBAL_INIT_PULSE;
    gimbal_ctrl.yaw_pulse   = GIMBAL_INIT_PULSE;

    while(1)
    {
        /* 等待信号量 (有控制操作) */
        sem_wait(&gimbal_ctrl.sem);

        pthread_mutex_lock(&gimbal_ctrl.lock);
        memset(ctrl_order, 0, sizeof(ctrl_order));
        memcpy(ctrl_order, gimbal_ctrl.order, strlen(gimbal_ctrl.order)); /* 获得云台转向指令 */
        pthread_mutex_unlock(&gimbal_ctrl.lock);

        /* 输出控制命令 */
        printf("ctrl order: %s\n", ctrl_order);

        /* 控制舵机方向 */
        if (strncmp(ctrl_order, GIMBAL_CTRL_MOVE_UP, strlen(ctrl_order)) == 0)      /* 向上 */
        {
            gimbal_move_up();
        }
        else if (strncmp(ctrl_order, GIMBAL_CTRL_MOVE_DOWN, strlen(ctrl_order)) == 0)  /* 向下 */
        {
            gimbal_move_down();
        }
        else if (strncmp(ctrl_order, GIMBAL_CTRL_MOVE_LEFT, strlen(ctrl_order)) == 0)  /* 向左 */
        {
            gimbal_move_left();
        }
        else if (strncmp(ctrl_order, GIMBAL_CTRL_MOVE_RIGHT, strlen(ctrl_order)) == 0)  /* 向右 */
        {
            gimbal_move_right();
        }
        else
        {
            printf("unknow app ctrl order");
        }
    }
}
